// --------------------------------------------------------------------------------------------------------------------
// <copyright file="DriveBoardTest.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Hardware.EV4.UnitTests
{
    using System;
    using System.Collections.Generic;
    using System.Threading;
    using Microsoft.Robotics.Hardware.DAC;
    using Microsoft.VisualStudio.TestTools.UnitTesting;
    using Test.Robotics.Hardware.DAC;

    /// <summary>
    /// Tests DriveBoard
    /// </summary>
    [TestClass]
    public class DriveBoardTest
    {
        /// <summary>
        /// Test DriveBoard.DriveBoard
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void DriveBoardContructorTest()
        {
            MockTransport mockTransport = new MockTransport(new byte[1024]);
            MockProtocol mockProtocol = new MockProtocol();

            DeviceConfiguration deviceConfig = new DeviceConfiguration();

            DriveBoard board = new DriveBoard(deviceConfig, mockTransport, mockProtocol, 1000);
            Assert.IsNotNull(board);
        }

        /// <summary>
        /// Test DriveBoard.CliffDetection(true)
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void DriveBoardEnableCliffDetectionTest()
        {
            MockTransport mockTransport = new MockTransport(new byte[1024]);
            MockProtocol mockProtocol = new MockProtocol();

            DeviceConfiguration deviceConfig = new DeviceConfiguration();

            DriveBoard board = new DriveBoard(deviceConfig, mockTransport, mockProtocol, 1000);

            board.EnableCliffDetection(true);
        }

        /// <summary>
        /// Test DriveBoard.DisableCliffDetection(false)
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void DriveBoardDisableCliffDetectionTest()
        {
            MockTransport mockTransport = new MockTransport(new byte[1024]);
            MockProtocol mockProtocol = new MockProtocol();

            DeviceConfiguration deviceConfig = new DeviceConfiguration();

            DriveBoard board = new DriveBoard(deviceConfig, mockTransport, mockProtocol, 1000);

            board.EnableCliffDetection(false);
        }

        /// <summary>
        /// Test DriveBoard.Initialize
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void InitializeDriveBoardTest()
        {
            MockTransport mockTransport = new MockTransport(new byte[1024]);
            MockProtocol mockProtocol = new MockProtocol();

            DeviceConfiguration deviceConfig = new DeviceConfiguration();

            DriveBoard board = new DriveBoard(deviceConfig, mockTransport, mockProtocol, 1000);

            board.InitializeDriveBoard();
        }

        /// <summary>
        /// Test DriveBoard.KeepAlive
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void KeepAliveTest()
        {
            MockTransport mockTransport = new MockTransport(new byte[1024]);
            MockProtocol mockProtocol = new MockProtocol();

            DeviceConfiguration deviceConfig = new DeviceConfiguration();

            DriveBoard board = new DriveBoard(deviceConfig, mockTransport, mockProtocol, 1000);

            board.KeepAlive();
        }

        /// <summary>
        /// Test DriveBoard.GenerateHeartbeatState
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void GenerateHeartbeatTest()
        {
            MockTransport mockTransport = new MockTransport(new byte[1024]);
            MockProtocol mockProtocol = new MockProtocol();

            DeviceConfiguration deviceConfig = new DeviceConfiguration();

            DriveBoard board = new DriveBoard(deviceConfig, mockTransport, mockProtocol, 1000);

            HeartbeatState heartbeatState = board.GenerateHeartbeatState();
        }

        /// <summary>
        /// Test extraction of message data and marshaling to heartbeat state.
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void ExtractAndMarshalHeartbeatTest()
        {
            // sample heartbeat taken from ev4a-007 while docked
            byte[] heartbeat = System.Convert.FromBase64String(
                "777//wGAogDdEvMBAAAAAAAAIAMAAM4CAAAgAwAAfwAmAikCZAAZAWQAAHwBJgAuACUAAPsAGQDeaOn/OwF" +
                "kAP//oECSE6cvAAA6PQAAxLwAQIE/aM6lvGsrqr1eWpS8eQ4QAFX9DgAAAACAAAAAAAAAAAAAAAAAwBAQBA" +
                "IAAAAAAAACAAAAAAAAAAAf3Z48AAAAABgHHQeamRk/mpkZP9gJnAkAIAIBAQFQia3e777//wGAogDhEvMBA" +
                "AAAAAAAIAMAACADAAAgAwAAfwArAiYCZAAZAWQAAHwBJgAuACUAAPsAGQDeaOn/OwFkAP//oECYE5kvAACs" +
                "vAAAhLwAgH0/XlqUvGsrqr1YoIu8eQ4QAFX9DgAAAACAAAAAAAAAAAAAAAAAwBAQBAIAAAAAAAACAAAAAAA" +
                "AAAAf3Z48AAAAABgHHQeamRk/mpkZP9gJnAkAIAIBAQF0Lq3e777//wGAogDlEvMBAAAAAAAAIAMAACADAA" +
                "AgAwAAfwAuAiYCZAAZAWQAAIEBJgAuACUAAPsAGQDeaOn/OwFkAP//oECYE5kvAABoPAAA3DwAQIA/boguv" +
                "G6Irr1S5oK8eQ4QAFX9DgAAAACAAAAAAAAAAAAAAAAAwBAQBAIAAAAAAAACAAAAAAAAAAAf3Z48AAAAABgH" +
                "HQeamRk/mpkZP9gJnAkAIAIBAQFKea3e777//wGAogDpEvMBAAAAAAAAIAMAACADAAAgAwAAfwA0AiYCZAA" +
                "YAWQAAIEBJgAuACUAAPsAGQDeaOn/OwFkAP//oECeE5kvAADAugAASLwAwIE/efw/vGjOpb2aWHS8eQ4QAF" +
                "X9DgAAAACAAAAAAAAAAAAAAAAAwBAQBAIAAAAAAAACAAAAAAAAAAAf3Z48AAAAABgHHQc9Chc/9igcP9gJn" +
                "AkAIAIBAQEL6K3e777//wGAogDtEvMBAAAAAAAAIAMAACADAAAgAwAAfwA0AigCZAAYAWQAAH4BJgAuACUA" +
                "APsAGQDeaOn/OwFkAP//oECeE5kvAACwuwAAnDwAwIA/j+RivO1ZrL1jFJ28eQ4QAFX9DgAAAACAAAAAAAA" +
                "AAAAAAAAAwBAQBAIAAAAAAAACAAAAAAAAAAAf3Z48AAAAABgHHQc9Chc/9igcP9gJnAkAIAIBAQHw6K3eAA" +
                "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA" +
                "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA" +
                "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA==");

            MockTransport mockTransport = new MockTransport(heartbeat);
            MockProtocol mockProtocol = new MockProtocol();

            DeviceConfiguration deviceConfig = new DeviceConfiguration();

            DriveBoard board = new DriveBoard(deviceConfig, mockTransport, mockProtocol, 1000);

            HeartbeatObserver observer = new HeartbeatObserver();
            board.Subscribe(observer);

            while (!observer.Next)
            {
                Thread.Sleep(100);
            }

            AccelerometerState accel = observer.Heartbeat.Accelerometers[0];
            Assert.IsTrue(0.019 - accel.AccelerationInG.X < 0.001);
            Assert.IsTrue(1.006 - accel.AccelerationInG.Y < 0.001);
            Assert.IsTrue(-0.005 - accel.AccelerationInG.Z < 0.001);

            BatteryState battery = observer.Heartbeat.Batteries[0];
            Assert.IsTrue(battery.BatteryCharging);
            Assert.AreEqual<int>(16544, battery.BatteryStatus);
            Assert.AreEqual<double>(100, battery.ChargePercentage);
            Assert.AreEqual<double>(-23, battery.CurrentInAmps);

            DifferentialDriveState diffDrive = observer.Heartbeat.DifferentialDrives[0];
            Assert.IsTrue(1052.281 - diffDrive.LeftWheelPositionInMeters < 0.001);
            Assert.IsTrue(982.357 - diffDrive.RightWheelPositionInMeters < 0.001);

            List<FanState> fans = observer.Heartbeat.Fans;
            Assert.AreEqual<int>(2520, fans[0].RotationsPerMinute);
            Assert.AreEqual<int>(2460, fans[1].RotationsPerMinute);

            GyroscopeState gyro = observer.Heartbeat.Gyroscopes[0];
            Assert.IsTrue(-0.084 - gyro.RotationalVelocityInRadiansPerSecond.X < 0.001);
            Assert.IsTrue(-0.019 - gyro.RotationalVelocityInRadiansPerSecond.Y < 0.001);
            Assert.IsTrue(-0.014 - gyro.RotationalVelocityInRadiansPerSecond.Z < 0.001);

            HumidityState humidity = observer.Heartbeat.HumiditySensors[0];
            Assert.AreEqual<double>(25, humidity.RelativeHumidityInPercentage);

            List<IRProximityState> irs = observer.Heartbeat.InfraredSensors;
            Assert.IsTrue(0.8 - irs[0].DistanceInMeters < 0.001);
            Assert.IsTrue(0.8 - irs[1].DistanceInMeters < 0.001);
            Assert.IsTrue(0.8 - irs[2].DistanceInMeters < 0.001);
            Assert.IsTrue(0.127 - irs[3].DistanceInMeters < 0.001);
            Assert.IsTrue(0.564 - irs[4].DistanceInMeters < 0.001);
            Assert.IsTrue(0.552 - irs[5].DistanceInMeters < 0.001);
            Assert.IsTrue(0.1 - irs[6].DistanceInMeters < 0.001);
            Assert.IsTrue(0.28 - irs[7].DistanceInMeters < 0.001);
            Assert.IsTrue(0.1 - irs[8].DistanceInMeters < 0.001);

            List<JointState> joints = observer.Heartbeat.Joints;
            Assert.IsTrue(0 - joints[0].PositionInRadians < 0.001);
            Assert.IsTrue(0.019 - joints[1].PositionInRadians < 0.001);

            PowerState power = observer.Heartbeat.PowerDevices[0];
            Assert.AreEqual<int>(4288, power.State);

            List<SonarState> sonars = observer.Heartbeat.Sonars;
            Assert.IsTrue(1.816 - sonars[0].DistanceInMeters < 0.001);
            Assert.IsTrue(1.821 - sonars[1].DistanceInMeters < 0.001);

            List<TemperatureState> temps = observer.Heartbeat.TemperatureSensors;
            Assert.IsTrue(31.5 - temps[0].TemperatureInDegreesCelsius < 0.001);
            Assert.IsTrue(38.2 - temps[1].TemperatureInDegreesCelsius < 0.001);
            Assert.IsTrue(25.1 - temps[2].TemperatureInDegreesCelsius < 0.001);
            Assert.IsTrue(37 - temps[3].TemperatureInDegreesCelsius < 0.001);
            Assert.IsTrue(38 - temps[4].TemperatureInDegreesCelsius < 0.001);
            Assert.IsTrue(46 - temps[5].TemperatureInDegreesCelsius < 0.001);
        }

        /// <summary>
        /// Class to observe and report on heartbeats.
        /// </summary>
        private class HeartbeatObserver : IObserver<HeartbeatState>
        {
            /// <summary>
            /// Gets a value indicating whether the stream has completed.
            /// </summary>
            /// <remarks>This should never happen - heartbeat is an infinite stream.</remarks>
            public bool Completed { get; private set; }

            /// <summary>
            /// Gets a value indicating whether there has been an error.
            /// </summary>
            public bool Error { get; private set; }

            /// <summary>
            /// Gets a value indicating whether a (next) state has been received.
            /// </summary>
            public bool Next { get; private set; }

            /// <summary>
            /// Gets the last received heartbeat state.
            /// </summary>
            public HeartbeatState Heartbeat { get; private set; }

            /// <summary>
            /// Notifies the observer that the provider has finished sending push-based notifications.
            /// </summary>
            public void OnCompleted()
            {
                this.Completed = true;
            }

            /// <summary>
            ///  Notifies the observer that the provider has experienced an error condition.
            /// </summary>
            /// <param name="error">An object that provides additional information about the error.</param>
            public void OnError(Exception error)
            {
                this.Error = true;
            }

            /// <summary>
            /// Called when a new HeartbeatState is ready.
            /// </summary>
            /// <param name="heartbeat">Latest HeartbeatState.</param>
            public void OnNext(HeartbeatState heartbeat)
            {
                this.Next = true;
                this.Heartbeat = heartbeat;
            }
        }
    }
}
